#include <tsubamegaeshi-base/worldmodel.h>
#include <iostream>

void
Worldmodel::TStatus::clear()
{
  time_           = 0;
  gs_time_        = 0;
  gs_pm_          = "";
  ball_           = TVector3(0, 0, 0);
  position_       = TVector3(0, 0, 0);
  position_mypos_ = TVector3(0, 0, 0);  // (mypos x y z) 
  zax_            = TVector3(0, 0, 0);
  
  teammate_.clear();
  opponent_.clear();

  hj_.clear(); 
  uj_.clear(); 
  flag_.clear(); 
  gyr_.clear();  
  frp_.clear();
}

void
Worldmodel::TStatus::dump()
{
  std::cout << "(TIME): " << time_ << std::endl;

  std::cout << "(POSITION): "
            << position_.v_[0] <<  ": "
            << position_.v_[1] <<  ": "
            << position_.v_[2] << std::endl;

  std::cout << "(MYPOS): "
            << position_mypos_.v_[0] <<  ": "
            << position_mypos_.v_[1] <<  ": "
            << position_mypos_.v_[2] << std::endl;

  std::cout << "(ZAXIS): "
            << zax_.v_[0] <<  ": "
            << zax_.v_[1] <<  ": "
            << zax_.v_[2] << std::endl;

  std::cout << "(BALL): "
            << ball_.v_[0] <<  ": "
            << ball_.v_[1] <<  ": "
            << ball_.v_[2] << std::endl;

  std::cout << "(GS): "
            << gs_time_ << ": "
            << gs_pm_ << ": "
            << std::endl;

  TGyrMap::const_iterator gyr_it = gyr_.begin();
  for (; gyr_it != gyr_.end(); gyr_it++) {
    std::cout << "(GYR): " << gyr_it->first << ": "
              << gyr_it->second.v_[0] <<  ": "
              << gyr_it->second.v_[1] <<  ": "
              << gyr_it->second.v_[2] << std::endl;
  }

  TFrpMap::const_iterator frp_it = frp_.begin();
  for (; frp_it != frp_.end(); frp_it++) {
    std::cout << "(FRP): "
              << frp_it->first << ": c( "
              << frp_it->second.c_.v_[0] <<  ", "
              << frp_it->second.c_.v_[1] <<  ", "
              << frp_it->second.c_.v_[2] <<  ") f( "
              << frp_it->second.f_.v_[0] <<  ", "
              << frp_it->second.f_.v_[1] <<  ", "
              << frp_it->second.f_.v_[2] <<  ")"
              << std::endl;
  }

  THJSenceMap::const_iterator hj_it = hj_.begin();
  for (; hj_it != hj_.end(); hj_it++) {
    std::cout << "(HJ): " << hj_it->first << ": " << hj_it->second << std::endl;
  }

  TUJSenceMap::const_iterator uj_it = uj_.begin();
  for (; uj_it != uj_.end(); uj_it++) {
    std::cout << "(UJ): " << uj_it->first << ": "
              << uj_it->second.first <<  ": "
              << uj_it->second.second << std::endl;
  }

  TFlagSenceMap::const_iterator f_it = flag_.begin();
  for (; f_it != flag_.end(); f_it++) {
    std::cout << "(F): " << f_it->first << ": "
              << f_it->second.v_[0] <<  ": "
              << f_it->second.v_[1] <<  ": "
              << f_it->second.v_[2] << std::endl;
  }

  TPlayerMap::const_iterator teammate_it = teammate_.begin();
  for (; teammate_it != teammate_.end(); teammate_it++) {
    std::cout << "(TEAMMATE): " << teammate_it->first << ": "
              << teammate_it->second.v_[0] <<  ": "
              << teammate_it->second.v_[1] <<  ": "
              << teammate_it->second.v_[2] << std::endl;
  }

  TPlayerMap::const_iterator opponent_it = opponent_.begin();
  for (; opponent_it != opponent_.end(); opponent_it++) {
    std::cout << "(OPPONENT): " << opponent_it->first << ": "
              << opponent_it->second.v_[0] <<  ": "
              << opponent_it->second.v_[1] <<  ": "
              << opponent_it->second.v_[2] << std::endl;
  }



}
